import sys
from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QLabel, QSlider, QScrollArea, QPushButton
from PyQt5.QtCore import Qt
from qt5.socketU import TCPClient

class SliderWindow(QWidget):
    # 将关节数据和默认值提取为类属性
    JOINT_DATA = [
        ("kLeftShoulderPitch", -3.0892, 2.6704),
        ("kLeftShoulderRoll", -1.5882, 2.2515),
        ("kLeftShoulderYaw", -2.618, 2.618),
        ("kLeftElbow", -1.0472, 2.0944),
        ("kLeftWristRoll", -1.9722, 1.9722),
        ("kLeftWristPitch", -1.6144, 1.6144),
        ("kLeftWristYaw", -1.6144, 1.6144),
        ("kRightShoulderPitch", -3.0892, 2.6704),
        ("kRightShoulderRoll", -2.2515, 1.5882),
        ("kRightShoulderYaw", -2.618, 2.618),
        ("kRightElbow", -1.0472, 2.0944),
        ("kRightWristRoll", -1.9722, 1.9722),
        ("kRightWristPitch", -1.6144, 1.6144),
        ("kRightWristYaw", -1.6144, 1.6144),
        ("kWaistYaw", -2.618, 2.618),
        ("kWaistRoll", -0.52, 0.52),
        ("kWaistPitch", -0.52, 0.52)
    ]
    DEFAULT_VALUES = [
        0.292139, 0.220713, -0.00117445, 0.977721, 0.050933, 0.0229714, -0.0037966,
        0.291504, -0.226418, 0.00385892, 0.977841, -0.101027, -0.060039, 0.0171883,
        -0.00121866, 0, 0
    ]

    def __init__(self):
        super().__init__()
        self.initUI()
        self.socket_client = TCPClient("192.168.110.252", 8080)
        self.socket_client.connect()
        self.changed_slider = {}

    def initUI(self):
        self.sliders = []  # 存储所有滑块的引用

        # 创建滚动区域
        scroll_area = QScrollArea()
        scroll_widget = QWidget()
        scroll_layout = QVBoxLayout(scroll_widget)

        # 为每个关节创建滑块和标签
        for i, (name, min_angle, max_angle) in enumerate(self.JOINT_DATA):
            label = QLabel(name)
            slider = QSlider(Qt.Horizontal)
            # 修改滑块范围为实际的最小和最大角度
            slider.setRange(int(min_angle * 1000), int(max_angle * 1000))  # 放大 1000 倍以支持小数
            # 设置滑块初始值
            default_value = self.DEFAULT_VALUES[i]
            # 确保默认值在滑块范围内
            if default_value < min_angle:
                default_value = min_angle
            elif default_value > max_angle:
                default_value = max_angle
            slider.setValue(int(default_value * 1000))

            slider.sliderReleased.connect(lambda n=name, s=slider, min=min_angle, max=max_angle: self.slider_released(n, s, min, max))
            slider.valueChanged.connect( lambda value, n=name, min=min_angle, max=max_angle: self.slider_changed(value / 1000, n, min, max))

            scroll_layout.addWidget(label)
            scroll_layout.addWidget(slider)
            self.sliders.append(slider)

        scroll_area.setWidget(scroll_widget)
        scroll_area.setWidgetResizable(True)

        # 添加两个按钮
        reset_button = QPushButton('恢复默认值')
        reset_button.clicked.connect(self.reset_sliders)

        print_button = QPushButton('打印值')
        print_button.clicked.connect(self.print_changed_sliders)

        main_layout = QVBoxLayout()
        main_layout.addWidget(scroll_area)
        main_layout.addWidget(reset_button)
        main_layout.addWidget(print_button)
        self.setLayout(main_layout)

        self.setWindowTitle('Joint Sliders')
        self.setGeometry(300, 300, 500, 500)
        self.show()

    def slider_released(self, name, slider, min_angle, max_angle):
        value = slider.value() / 1000
        command = f"set {name} = {value}"
        print(f"-----command: {command}")
        self.socket_client.send_data(command)

    def slider_changed(self, value, name, min_angle, max_angle):
        self.changed_slider[name] = value

    def reset_sliders(self):
        self.changed_slider = {}
        for i, slider in enumerate(self.sliders):
            default_value = self.DEFAULT_VALUES[i]
            name, min_angle, max_angle = self.JOINT_DATA[i]
            if default_value < min_angle:
                default_value = min_angle
            elif default_value > max_angle:
                default_value = max_angle
            slider.setValue(int(default_value * 1000))

    def print_changed_sliders(self):
        all_values = []
        for i, slider in enumerate(self.sliders):
            all_values.append(slider.value() / 1000)
        print(f"-----all_values: {all_values}")

if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = SliderWindow()
    sys.exit(app.exec_())